#include <ros/ros.h>
#include <liblas/liblas.hpp>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <cmath>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <string>

#include <cstdlib>
#include <iostream>

// 文件夹名称
std::string input_pcd_folder_path;
std::string output_las_folder_path;
// pcd文件名
// std::string input_pcd_name;

//  实现Unshort转为Unchar类型
// int Unshort2Unchar(uint16_t &green, uint8_t &g)
//{
//    unsigned short * word;
//    word = &green;
//    int size = WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, NULL, 0, NULL, FALSE);
//    char *AsciiBuff=new char[size];
//    WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, AsciiBuff, size, NULL, FALSE);
//    g = *AsciiBuff;
//
//    delete[] AsciiBuff;
//    AsciiBuff = NULL;
//    return 0;
//}

void writePointCloudFromLas(const char *strInputLasName, const char *strOutPutPointCloudName)
{
    // 打开las文件
    std::ifstream ifs;
    ifs.open(strInputLasName, std::ios::in | std::ios::binary);
    if (!ifs.is_open())
    {
        std::cout << "无法打开.las" << std::endl;
        return;
    }
    liblas::ReaderFactory readerFactory;
    liblas::Reader reader = readerFactory.CreateWithStream(ifs);
    // 写点云
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOutput(new pcl::PointCloud<pcl::PointXYZRGBA>());
    cloudOutput->clear();
    while (reader.ReadNextPoint())
    {
        double x = reader.GetPoint().GetX();
        double y = reader.GetPoint().GetY();
        double z = reader.GetPoint().GetZ();
        uint16_t red = reader.GetPoint().GetColor()[0];
        uint16_t green = reader.GetPoint().GetColor()[1];
        uint16_t blue = reader.GetPoint().GetColor()[2];

        /*****颜色说明
         *   uint16_t  范围为0-256*256 ；
         *   pcl 中 R  G  B利用的unsigned char  0-256；
         *   因此这里将uint16_t 除以256  得到  三位数的0-256的值
         *   从而进行rgba 值32位 的位运算。
         *
         ******/

        pcl::PointXYZRGBA thePt; // int rgba = 255<<24 | ((int)r) << 16 | ((int)g) << 8 | ((int)b);
        thePt.x = x;
        thePt.y = y;
        thePt.z = z;
        thePt.rgba = (uint32_t)255 << 24 | (uint32_t)(red / 256) << 16 | (uint32_t)(green / 256) << 8 | (uint32_t)(blue / 256);
        // uint32_t rgb = *reinterpret_cast<int*>(&thePt.rgb);  //reinterpret_cast 强制转换
        cloudOutput->push_back(thePt);
    }

    // boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    // viewer->setBackgroundColor(0, 0, 0); //设置背景
    // viewer->addPointCloud(cloudOutput,"cloudssd");
    // while (!viewer->wasStopped()){
    //     viewer->spinOnce();
    // }
    pcl::io::savePCDFileASCII(strOutPutPointCloudName, *cloudOutput);
    cloudOutput->clear();
}

// 读入点云，写.las

void writeLasFromPointCloud_XYZI(const std::string strInputPointCloudName, std::string strOutLasName)
{
    std::ofstream ofs(strOutLasName.c_str(), ios::out | ios::binary);
    if (!ofs.is_open())
    {
        std::cout << "err  to  open  file  las....." << std::endl;
        return;
    }
    liblas::Header header;
    header.SetVersionMajor(1);
    header.SetVersionMinor(2);
    header.SetDataFormatId(liblas::ePointFormat3);
    header.SetScale(0.001, 0.001, 0.001);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(strInputPointCloudName, *cloud);
    std::cout << "总数:" << cloud->points.size() << std::endl;
    // 写liblas,
    liblas::Writer writer(ofs, header);
    liblas::Point point(&header);

    for (size_t i = 0; i < cloud->points.size(); i++)
    {
        double x = cloud->points[i].x;
        double y = cloud->points[i].y;
        double z = cloud->points[i].z;
        point.SetCoordinates(x, y, z);

        //        uint32_t red = (uint32_t)cloud->points[i].r;
        //        uint32_t green = (uint32_t)cloud->points[i].g;
        //        uint32_t blue = (uint32_t)cloud->points[i].b;
        //        liblas::Color pointColor(red, green, blue);
        //        point.SetColor(pointColor);
        writer.WritePoint(point);
        // std::cout << x << "," << y << "," << z << std::endl;
    }
    double minPt[3] = {9999999, 9999999, 9999999};
    double maxPt[3] = {0, 0, 0};
    header.SetPointRecordsCount(cloud->points.size());
    header.SetPointRecordsByReturnCount(0, cloud->points.size());
    header.SetMax(maxPt[0], maxPt[1], maxPt[2]);
    header.SetMin(minPt[0], minPt[1], minPt[2]);
    writer.SetHeader(header);
}

void writeLasFromPointCloud_XYZRGB(const std::string strInputPointCloudName, std::string strOutLasName)
{
    std::ofstream ofs(strOutLasName.c_str(), ios::out | ios::binary);
    if (!ofs.is_open())
    {
        std::cout << "err  to  open  file  las....." << std::endl;
        return;
    }
    liblas::Header header;
    header.SetVersionMajor(1);
    header.SetVersionMinor(2);
    header.SetDataFormatId(liblas::ePointFormat3);
    header.SetScale(0.001, 0.001, 0.001);

    // 设置空间参考系统为 "WGS 84"
    liblas::SpatialReference srs;
    srs.SetProj4("+proj=longlat +datum=WGS84 +no_defs");
    header.SetSRS(srs);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile(strInputPointCloudName, *cloud);
    std::cout << "总数:" << cloud->points.size() << std::endl;
    // 计算点云的最小和最大坐标
    double minPt[3] = {std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max()};
    double maxPt[3] = {-std::numeric_limits<double>::max(), -std::numeric_limits<double>::max(), -std::numeric_limits<double>::max()};
    // 写liblas,
    liblas::Writer writer(ofs, header);
    liblas::Point point(&header);
    for (size_t i = 0; i < cloud->points.size(); i++)
    {
        double x = cloud->points[i].x;
        double y = cloud->points[i].y;
        double z = cloud->points[i].z;
        point.SetCoordinates(x, y, z);
        uint32_t red = (uint32_t)cloud->points[i].r;
        uint32_t green = (uint32_t)cloud->points[i].g;
        uint32_t blue = (uint32_t)cloud->points[i].b;
        liblas::Color pointColor(red, green, blue);
        point.SetColor(pointColor);
        writer.WritePoint(point);
    }
    header.SetPointRecordsCount(cloud->points.size());
    header.SetPointRecordsByReturnCount(0, cloud->points.size());
    header.SetMax(maxPt[0], maxPt[1], maxPt[2]);
    header.SetMin(minPt[0], minPt[1], minPt[2]);
    writer.SetHeader(header);
    ROS_WARN("write las file success");
}

int main(int argc, char **argv)
{
    std::cout << "Starting pcd2las node..." << std::endl;
    // 初始化ROS节点
    ros::init(argc, argv, "pcd2las_node");

    // 创建ROS节点句柄
    ros::NodeHandle nh("~");
    std::vector<std::string> pcd_files;
    nh.getParam("input_pcd_folder_path", input_pcd_folder_path);
    nh.getParam("output_las_folder_path", output_las_folder_path);
    // ROS_INFO("input_pcd_folder_path: %s", input_pcd_folder_path.c_str());
    // ROS_INFO("output_las_folder_path: %s", output_las_folder_path.c_str());
    // 遍历文件夹，文件夹名称
    std::string pcd_folder_path = std::getenv("HOME") + input_pcd_folder_path;
    ROS_INFO("pcd_folder_path: %s", pcd_folder_path.c_str());
    // 检测文件夹是否存在
    if (!boost::filesystem::exists(pcd_folder_path))
    {
        ROS_ERROR("pcd folder not exist");
        return 0;
    }
    // 最新文件名
    std::string latest_pcd_file;
    std::chrono::system_clock::time_point latest_time;
    ros::Rate loop_rate(0.1);
    while (ros::ok())
    {
        // 遍历文件夹，存储所有pcd文件名
        for (const auto &file : boost::filesystem::directory_iterator(pcd_folder_path))
        {
            if (file.path().extension() == ".pcd")
            {
                pcd_files.push_back(file.path().string());
                // ROS_INFO("pcd file: %s", file.path().string().c_str());
                std::string pcd_name = file.path().stem().string();
                // 获取文件的最后修改时间
                std::time_t file_time_t = boost::filesystem::last_write_time(file.path());
                auto file_time = std::chrono::system_clock::from_time_t(file_time_t);
                if (latest_pcd_file.empty() || file_time > latest_time)
                {
                    latest_time = file_time;
                    latest_pcd_file = pcd_name;
                }
            }
        }
        // ROS_INFO("pcd files size: %d", pcd_files.size());
        // ROS_INFO("latest pcd file: %s", latest_pcd_file.c_str());
        if (!latest_pcd_file.empty())
        {
            // 读取最新pcd文件
            std::string strInputPointCloudName = pcd_folder_path + "/" + latest_pcd_file + ".pcd";
            std::string strOutLasName = std::getenv("HOME") + output_las_folder_path + "/" + latest_pcd_file + ".las";
            ROS_INFO("input pcd file: %s", strInputPointCloudName.c_str());
            // writeLasFromPointCloud_XYZI(strInputPointCloudName.c_str(), strOutLasName.c_str());
            writeLasFromPointCloud_XYZRGB(strInputPointCloudName.c_str(), strOutLasName.c_str());
        }
        else
        {
            ROS_WARN("No PCD files found in the specified folder.");
        }
        // 存贮转换后las文件夹的名
        std::string las_folder_path = std::getenv("HOME") + output_las_folder_path;
        // 读取转换后的las文件
        std::string SentLasName = las_folder_path + "/" + latest_pcd_file + ".las";
        std::string destination = "ubuntu@82.157.253.245:/home/ubuntu/cloudData/";
        std::string command = "sshpass -p '9;s/6xB7Q2mZ~'  rsync -avz " + SentLasName + " " + destination;
        // 使用 system() 函数调用 rsync 命令
        int result = std::system(command.c_str());
        // 检查命令是否成功执行
        if (result == !0)
        {
            ROS_ERROR("rsync failed");
        }

        /* ROS_INFO("SentLasName: %s", SentLasName.c_str());
        // 执行传输命令rsync -avz /mnt/d/rsync/TEST.txt  ubuntu@82.157.253.245:/home/ubuntu/cloudData/，并且输入密码9;s/6xB7Q2mZ~
        std::string rsync_cmd ="cd &&rsync -avz " + SentLasName + " " + "ubuntu@82.157.253.245:/home/ubuntu/cloudData/ && echo '9;s/6xB7Q2mZ~' | sudo -S ";
        ROS_INFO("rsync_cmd: %s", rsync_cmd.c_str());
        system(rsync_cmd.c_str()); */

        ros::spinOnce();
        loop_rate.sleep();
    }

    // las转pcd
    // char strInputLasName[] = "qq.las";//"Scan_az001.las";
    // char strOutPutPointCloudName[]="qqqq.pcd";
    // writePointCloudFromLas(strInputLasName, strOutPutPointCloudName);
}